/*
 * FDAnalysis.c
 * Copyright (C) 2013 Sleepwalking
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name ``Sleepwalking'' nor the name of any other
 *    contributor may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * CVEDSP IS PROVIDED BY Sleepwalking ``AS IS'' AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL Sleepwalking OR ANY OTHER CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include "BaseFreq.h"
#include "DSPBase/Filter.h"
#include "DSPBase/Spectrum.h"
#include "DSPBase/Window.h"
#include "DFT/FFT.h"
#include "IntrinUtil/Calculation.h"
#include "IntrinUtil/FloatArray.h"
#include "../Plot.h"
#include "../RUtil/Misc/Math.h"
#include <math.h>
#include <malloc.h>

#define FreqToIndex(Freq) (Freq * Amount / SampleRate)
#define IndexToFreq(Index) ((float)SampleRate * Index / Amount)

float Window[16384];
float Spec[16384];

BaseFreqAnalyzerParameters BPara;
/*  Original Condition
    BPara.Forward = 2;
    BPara.Backward = 2;
    BPara.AvgFactor = 1;
    BPara.MinFactor = 0.5;
    BPara.Start = 2;
*/
void BFAnalyzer_Init()
{
    //Generated by Genetic Optimizer
    BPara.Forward = 3;
    BPara.Backward = 4;
    BPara.AvgFactor = 1.186901;
    BPara.MinFactor = 0.342386;
    BPara.Start = 1;
}

void BFAnalyzer_SetParameters(BaseFreqAnalyzerParameters Src)
{
    BPara = Src;
}

float GetBaseFrequencyFromSpectrum(float* Spectrum, float LFreq, float UFreq, int Amount)
{
	int LBoundIndex, UBoundIndex;
    int i, j;
	float Avg;
	LBoundIndex = FreqToIndex(LFreq);
	UBoundIndex = FreqToIndex(UFreq);
    Avg = Boost_FloatAvg(Spectrum + LBoundIndex, UBoundIndex - LBoundIndex) * BPara.AvgFactor;
    for(i = LBoundIndex > BPara.Start ? LBoundIndex : BPara.Start; i < UBoundIndex; i ++)
	{
		if(Spectrum[i] > Avg)
        {
            for(j = i - 1; j >= i - BPara.Backward; j --)
                if(Spectrum[j] > Spectrum[i])
                    goto EXITFOR;
            for(j = i + 1; j <= i + BPara.Forward; j ++)
                if(Spectrum[j] > Spectrum[i])
                    goto EXITFOR;
            if(Boost_FloatMin(Spectrum, i - BPara.Backward, i) > Spectrum[i] * BPara.MinFactor)
                continue;
            if(Boost_FloatMin(Spectrum, i, i + BPara.Forward) > Spectrum[i] * BPara.MinFactor)
                continue;
            return IndexToFreq(i);
		}
        EXITFOR:;
	}
	return 0;
}

float GetBaseFrequencyFromSpectrum2(float* Spectrum, float LFreq, float UFreq, int Amount)
{
    int LBoundIndex, UBoundIndex;
    int i;
    float Threshold;
    LBoundIndex = FreqToIndex(LFreq);
    UBoundIndex = FreqToIndex(UFreq);
    float* Temp = (float*)malloc(sizeof(float) * UBoundIndex);
    Boost_FloatCopy(Temp, Spectrum, UBoundIndex);
    Math_FloatDecSort(Temp, UBoundIndex);
    Threshold = Temp[UBoundIndex / 20 > 10 ? UBoundIndex / 20 : 10];
    free(Temp);

    int Estimated = 0;
    for(i = LBoundIndex > 3 ? LBoundIndex : 3; i < UBoundIndex; i ++)
        if(Spectrum[i] > Threshold)
        {
            float Convex = Spectrum[i] * 2 - Spectrum[i - 3] - Spectrum[i + 3];
            if(Convex > Threshold * 1.5)
            {
                Estimated = i;
                break;
            }
        }
    if(Estimated < 3)
        return 0;
    float Max = 0;
    int Record = 0;
    for(i = Estimated - 3; i < Estimated + 5; i ++)
        if(Spectrum[i] > Max)
        {
            Max = Spectrum[i];
            Record = i;
        }
    return IndexToFreq(Record);
}

float GetBaseFrequencyFromCepstrum(float* Cepstrum, float LFreq, float UFreq, int Amount)
{
	return (float)SampleRate / GetBasePeriodFromCepstrum(Cepstrum,
	                                                     (float)SampleRate / UFreq,
	                                                     (float)SampleRate / LFreq,
	                                                     Amount);
}

int GetBasePeriodFromCepstrum(float* Cepstrum, float LPeriod, float UPeriod, int Amount)
{
	int i;
	float max = 0;
	int maxIndex = 0;
    UPeriod = UPeriod > Amount ? Amount : UPeriod;
    for(i = LPeriod; i < UPeriod; i ++)
	{
		if(Cepstrum[i] > max)
		{
			max = Cepstrum[i];
			maxIndex = i;
		}
	}
	return maxIndex;
}

float GetBaseFrequencyFromWave(float* Wave, float LFreq, float UFreq, int Power)
{
    int Amount;
	if(Power < 9 || Power > 14)
		return 0;
    Amount = pow(2, Power);
    float* Temp = (float*)malloc(sizeof(float) * Amount);

    GenerateHanning(Window, Amount);
    Boost_FloatMulArr(Temp, Wave, Window, Amount);
    MagnitudeFromWave(Spec, Temp, Power);

    free(Temp);
    return GetBaseFrequencyFromSpectrum2(Spec, LFreq, UFreq, Amount);
}
